/*
Robotics object oriented package in C++
Copyright (C) 2008-2009  Matrix

This library is free software; you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as
published by the Free Software Foundation; either version 2.1 of the
License, or (at your option) any later version.

This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston,  MA 02111-1307  USA
*/

#include "stdafx.h"
#include "Con_ComputedTorgue.h"

namespace RobotPlant { namespace Mechanism{

	using namespace std;
	using namespace RobotPlant::Math;

	Con_ComputedTorgue::Con_ComputedTorgue(const Chain* pRobot, const ColumnVector & Kp, const ColumnVector & Kd)
		//! @brief Constructor.
	{
		_pRobot = pRobot;
		_Kd = Matrix(_pRobot->Get_Dof(),_pRobot->Get_Dof());
		_Kp = Matrix(_pRobot->Get_Dof(),_pRobot->Get_Dof());
		_Kd.Identity();
		_Kp.Identity();

		Set_Kd(Kd);
		Set_Kp(Kp);
	}

	ColumnVector Con_ComputedTorgue::Cmd_JointAcc(const ColumnVector & qd, const ColumnVector & qpd, const ColumnVector & qppd)
	{
		if(qd.GetCount() != _pRobot->Get_Dof())
		{
			throw(new exception(" Argument qd's row number should be the same as robot dof."));
		}
		if(qpd.GetCount() != _pRobot->Get_Dof())
		{
			throw(new exception(" Argument qpd's row number should be the same as robot dof."));
		}
		if(qppd.GetCount() != _pRobot->Get_Dof())
		{
			throw(new exception(" Argument qppd's row number should be the same as robot dof."));
		}

		ColumnVector q = _pRobot->Get_JointValues();
		ColumnVector qp = _pRobot->Get_JointVels();
		ColumnVector qpp = _Kp*(qd - q) + _Kd*(qpd - qp) + qppd;

		return qpp;
	}

	ColumnVector Con_ComputedTorgue::Cmd_JointTorque(const ColumnVector & qd, const ColumnVector & qpd, const ColumnVector & qppd, const Vector3 & grav)
	{
		ColumnVector qpp = Cmd_JointAcc(qd, qpd, qppd);

		Vector3 zero3 = Vector3::Zero();
		Wrench wrench = Wrench(zero3, zero3);
		return _pRobot->InverseDyn_GearBoxTorque(_pRobot->Get_JointValues(), _pRobot->Get_JointVels(), qpp, wrench, grav);
	}

	void Con_ComputedTorgue::Set_Kd(const ColumnVector & Kd)
	{
		if(Kd.GetCount() != _pRobot->Get_Dof())
		{
			throw(new exception(" Argument _Kd's count number should be the same as _dof."));
		}

		_Kd.SetZero();
		for(int i=0; i<_pRobot->Get_Dof(); i++)
			_Kd(i,i) = Kd[i];
	}

	void Con_ComputedTorgue::Set_Kp(const ColumnVector & Kp)
	{
		if(Kp.GetCount() != _pRobot->Get_Dof())
		{
			throw(new exception(" Argument _Kp's count number should be the same as _dof."));
		}

		_Kp.SetZero();
		for(int i=0; i<_pRobot->Get_Dof(); i++)
			_Kp(i,i) = Kp[i];
	}


}}